# FMCW雷达信号处理传统算法类
import numpy as np

class FmcwRsp(object):
    def __init__(self):
        self.name = 'apps.fmcw.fmcw_rsp.FmcwRsp'
    
    @staticmethod
    def ca_cfar(RDM_dB, numGuard, numTrain, P_fa, SNR_OFFSET):
        numTrain2D = numTrain * numTrain - numGuard * numGuard
        RDM_mask = np.zeros(RDM_dB.shape)

        for r in range(numTrain + numGuard, RDM_mask.shape[0] - (numTrain + numGuard)):
            for d in range(numTrain + numGuard, RDM_mask.shape[1] - (numTrain + numGuard)):
                
                Pn = (np.sum(RDM_dB[r-(numTrain+numGuard):r+(numTrain+numGuard), d-(numTrain+numGuard):d+(numTrain+numGuard)]) - 
                    np.sum(RDM_dB[r-numGuard:r+numGuard, d-numGuard:d+numGuard])) / numTrain2D
                a = numTrain2D * (P_fa**(-1/numTrain2D) - 1)
                threshold = a * Pn
                if RDM_dB[r, d] > threshold and RDM_dB[r, d] > SNR_OFFSET:
                    RDM_mask[r, d] = 1

        # figure(2)
        # imagesc(RDM_mask)
        # title('CA-CFAR')

        cfar_ranges, cfar_dopps = np.where(RDM_mask == 1)  # cfar detected range bins

        # remaining part is for target location estimation
        rem_range = np.zeros(len(cfar_ranges), dtype=int)
        rem_dopp = np.zeros(len(cfar_dopps), dtype=int)
        for i in range(1, len(cfar_ranges)):
            if abs(cfar_ranges[i] - cfar_ranges[i-1]) <= 5 and abs(cfar_dopps[i] - cfar_dopps[i-1]) <= 5:
                rem_range[i] = i  # redundant range indices to be removed
                rem_dopp[i] = i  # redundant doppler indices to be removed

        rem_range = rem_range[rem_range != 0]  # filter zeros
        rem_dopp = rem_dopp[rem_dopp != 0]  # filter zeros
        cfar_ranges = np.delete(cfar_ranges, rem_range)
        cfar_dopps = np.delete(cfar_dopps, rem_dopp)
        K = len(cfar_dopps)  # # of detected targets

        return RDM_mask, cfar_ranges, cfar_dopps, K